package robocup.component.worldmodel;

import java.util.LinkedList;
import java.util.Vector;

import robocup.component.DAInamiteConstants;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.actions.Action;
import robocup.component.actions.MoveAction;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.BallInfo;
import robocup.component.infotypes.CoachSayInfo;
import robocup.component.infotypes.InfoReceiver;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.PlayerTypesInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.speechacts.MessageFactory;
import robocup.component.speechacts.MessageFactory.MESSAGES;
import robocup.component.worldmodel.particlefilter.BallParticleFilter;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.Ball.UPDATE_MODE;
import robocup.component.worldobjects.Player.RELATIVE;

/**
 * BallModel is the part of the WorldModel which implements functions to
 * calculate the state of the ball (position, speed, direction, ...). It is
 * updated each time an info arrives. As seen data is given relative to the
 * players position and headdir the {@link MeModel} update has to be invoked
 * before.
 */
public class BallModel
    implements InfoReceiver {

  // the world for other worldmodel parts methods calls
  private final WorldModel         world;

  // The ball represented by this BallModel
  private final Ball               ball;

  // the ball as it was last seen
  private final Ball               lastSeenBall     = Ball.getDummyInstance();

  // the particle filter used for calculating the balls speed and position
  private final BallParticleFilter particleFilter   = new BallParticleFilter();

  // use the filter
  private final boolean            ParticlefilterOn = false;

  /**
   * Constructor for BallModel - an object holding the actual ball of the
   * worldmodel and providing the update logic and some useful functions over
   * the ball.
   * 
   * @param -
   *          the current world-model (parent)
   */
  public BallModel(
      final WorldModel world) {

    this.world = world;
    this.ball = Ball.getDummyInstance();
  }

  /**
   * updateBallVisual updates the Ball <code>ball</code> from the VisualInfo
   * <code>vInfo</code>. If the ball was not seen, nothing is done as it was
   * already updated by prediction during the BS update.
   * 
   * @param info -
   *          the visual info
   */
  public void update(final VisualInfo info) {

    final BallInfo bi = info.getBall();

    if (bi == null) {
      /**
       * the player doesn't see the ball, we keep its pos/speed calculated in
       * update(SBInfo) method.
       * 
       * if the Ball isn't seen but should be, it isn't reliable anymore
       */
      if (this.world.shouldBeSeen(this.ball)) {
        this.ball.setReliable(false);
        this.ball.setSpeedReliable(false);
        this.ball.getSpeed().reset();
      }
      return;
    }

    // the ball pos (relative)
    final Vektor ballPosRel = new Vektor(bi.getDistance(), bi.getDirection());

    // the ball pos (absolute)
    Vektor ballPosAbs = this.world.getSelfRef().getPosAbs(ballPosRel,
        RELATIVE.TO_HEAD);

    // if someone is dribbling we already know the ball as it was communicated
    // we believe more in the heared info instead of the seen one as we assume
    // that the dribbling player is nearer to the ball having less noisy
    // information
    if (ballPosRel.getLength() > 3 && this.world.getSelfRef()
        .getLastHearBodyCycle() == this.world.getBodyCycle() && this.world
        .getSelfRef().getLastHearedMessageType() == MESSAGES.DRIBBLE_PPOS && ballPosAbs
        .getDistance(this.ball.getPosition()) < ballPosRel.getLength() * 0.1 + 1.2) {
      return;
    }

    // if someone is passing we already know the ball as well
    if (ballPosRel.getLength() > 10 && this.world.getSelfRef()
        .getLastHearBodyCycle() == this.world.getBodyCycle() && this.world
        .getSelfRef().getLastHearedMessageType() == MESSAGES.KICKED_BALL && ballPosAbs
        .getDistance(this.ball.getPosition()) < ballPosRel.getLength() * 0.1 + 1.2) {
      return;
    }

    Vektor ballSpeed = new Vektor();
    boolean isSpeedReliable = true;

    // the velocity is written into ballSpeed
    this.world.calculateVelocity(bi, ballSpeed);

    // if the ball's velocity is high we must be in PLAY_MODE=PLAY_ON
    // as we sometimes miss play_mode change infos we set this here
    if (!ballSpeed.isEmpty() && this.world.getPlayMode() != PLAY_MODE.PLAY_ON && ballSpeed
        .getLength() > 1.0) {
      if (!PLAY_MODE.META_PENALTY_OWN.contains(this.world.getPlayMode()) && !PLAY_MODE.META_PENALTY_OTHER
          .contains(this.world.getPlayMode())) {
        this.world.correctPlayMode(PLAY_MODE.PLAY_ON);
      }
    }

    // if the ball is far or behind us there might not be seen it's velocity
    // the velocity is there approximated as the way between new and last seen
    // position divide by the number of cycles the ball was not seen corrected
    // by the fact that the ball speed decays
    if (ballSpeed.isEmpty() && (this.world.getPlayMode() == PLAY_MODE.PLAY_ON || (this.world
        .getPlayMode() == PLAY_MODE.GOAL_KICK_OWN && this.world.getNumber() == 1)) && !this.world
        .collided()) {

      isSpeedReliable = false;
      ballSpeed.copy(ballPosAbs);
      ballSpeed.subFromThis(this.lastSeenBall.getPosition());

			final int timeDiff = this.world.getBodyCycle()
					- this.lastSeenBall.getBodyCycle();


      if (timeDiff == 1) {
        ballSpeed.subFromThis(this.world.getSelfPositionEstimationError());
      }

			final double way = ballSpeed.getLength();
			double arrivalSpeed = (way / Ball.relativeBallMovementAfterNCycles(timeDiff))
					* Ball.ballDecayAfterNCycles(timeDiff);


      arrivalSpeed = Math.min(SConf.getInstance().BALL_SPEED_MAX, arrivalSpeed);

      ballSpeed.setLength(arrivalSpeed);
    }

    // we collided, or the play_mode isn't PLAY_ON -> the speed was already set
    // after the BSInfo
    if (ballSpeed.isEmpty()) {
      ballSpeed.copy(this.ball.getSpeed());
      isSpeedReliable = false;
    }

    // add ball to particle filter
    if (this.ParticlefilterOn && !(!isSpeedReliable && ballPosRel.getLength() < 3) && this.world
        .getPlayMode() == PLAY_MODE.PLAY_ON) {
      this.particleFilter.addVI(bi, this.world.getSelfRef().getHeadDir(),
          this.world.getMyPosition(), this.world.getSelfRef().getSpeed());
      ballPosAbs = this.particleFilter.getPosition();
      if (this.particleFilter.isSpeedInitialized()) {
        ballSpeed = this.particleFilter.getSpeed();
      }
    }

    // if the ball was seen slightly outside due to noise we set it into the
    // field preventing some of the other methods doing strange things
    this.world.setIntoField(ballPosAbs);
    if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON) {
      ballSpeed.reset();
      isSpeedReliable = true;
    }

    // update the ball
    this.ball.setPosition(ballPosAbs);
    this.ball.setSpeed(ballSpeed);
    this.ball.setCycle(this.world.getCycle());
    this.ball.setBodyCycle(this.world.getBodyCycle());
    this.ball.setReliable(true);
    this.ball.setSpeedReliable(isSpeedReliable);

    // set new age, full seen balls include half seen ones
    if (isSpeedReliable) {
      this.ball.setUpdateMode(UPDATE_MODE.SEEN_FULL, this.world.getBodyCycle());
      this.ball.setUpdateMode(UPDATE_MODE.SEEN_WITHOUT_SPEED, this.world
          .getBodyCycle());
    }
    else {
      this.ball.setUpdateMode(UPDATE_MODE.SEEN_WITHOUT_SPEED, this.world
          .getBodyCycle());
    }

  }

  /**
   * updateBallSense updates the Ball <code>ball</code> from the SenseBodyInfo
   * <code>sbInfo</code>
   * 
   * @param info -
   *          Info for the update
   */
  public void update(final SenseBodyInfo info) {

    // ball was seen in the last cycle -> save it
    if (this.world.getAge(this.ball) == 1) {
      this.lastSeenBall.copy(this.ball);
    }

    // no play_on -> no ball update as we prefer waiting for a visual info
    if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON) {
      return;
    }

    if (this.ParticlefilterOn) {
      this.particleFilter.translatePosition();
    }

    final Vector<Action> sentActions = info.getLastSentActions();

    if (sentActions == null) {
      return;
    }

    boolean kicked = false;

    // predict the ball based on the last world and the actions done
    // only kick, tackle, catch and move(goalie) actions have an effect
    for (final Action action : sentActions) {

      switch (action.getType()) {

        case KICK:
        case CATCH_BALL:
        case TACKLE:
          final Player lastMe = this.world.getLastMe();
          this.ball.predictThisAfterAction(lastMe, action);
          if (this.ParticlefilterOn) {
            this.particleFilter.resetPosition(this.ball.getPosition(),
                this.ball.getSpeed());
          }
          kicked = true;
          break;

        case MOVE:
          if (this.world.getSelfRef().isGoalie() && this.ball.isCatched()) {
            // if the goalie moves with the ball -> move the ball
            final MoveAction ma = (MoveAction) action;
            this.ball
                .setPosition(new Vektor(
                    Vektor.XY,
                    ma.getX(),
                    ma.getY() + this.world.getSelfRef().getPConf().PLAYER_SIZE + SConf
                        .getInstance().BALL_SIZE));
            this.ball.getSpeed().reset();
            break;

          }
        default:
          ;
      }
    }

    // update ball if not kicked using the filter or simple extrapolation
    if (this.ParticlefilterOn && !kicked && this.particleFilter
        .isSpeedInitialized()) {
      this.ball.setPosition(this.particleFilter.getPosition());
      this.ball.setSpeed(this.particleFilter.getSpeed());
    }
    else if (!kicked && !this.world.ballCanBeKickedByOther()) {
      this.ball.extrapolate(1);
    }

    // if visualAge reached DAInamiteConstants.WM_V_AGE_CONFIDENCE, the ball
    // isn't reliable anymore.
    if (this.world.getAge(this.ball) > DAInamiteConstants.WM_V_AGE_CONFIDENCE) {
      this.ball.setReliable(false);
      this.ball.getSpeed().reset();
      this.ball.setSpeedReliable(false);
    }
    this.ball.setUpdateMode(UPDATE_MODE.EXPECTED, this.world.getBodyCycle());
  }

  /**
   * This method handles Say-Messages from other players.
   * 
   * There are many message types containing ball information. This information
   * is important as it is probably of higher quality as only players near to
   * the ball and/or handling the ball will communicate it.
   * 
   * @param psi -
   *          the {@link PlayerSayInfo}
   */
  public void update(final PlayerSayInfo psi) {

    // our ball data is better than the communicated info -> don't update
    if ((this.ball.getAge(this.world.getBodyCycle(),
        UPDATE_MODE.META_BASED_ON_INFO) == 0 && this.world
        .getDistance(this.ball) < 20) || (this.world.getDistance(this.ball) < 3 && this.ball
        .isReliable())) {
      return;
    }

    final MessageFactory mf = this.world.getMessageFactory();
    final LinkedList<MESSAGES> types = mf.getMessageTypes();

    for (int i = 0; i < types.size(); i++) {
      final MESSAGES m = types.get(i);

      switch (m) {
        // the ball was kicked by the sender -> always update
        case KICKED_BALL:
          mf.decodeMessageNumber(i);
          this.ball.getPosition().copy(mf.getPosition());
          this.ball.getSpeed().copy(mf.getSpeed());
          this.ball.setReliable(true);
          this.ball.setSpeedReliable(true);
          this.ball.setBodyCycle(this.world.getBodyCycle());
          this.ball.setCycle(this.world.getCycle());
          if (this.ParticlefilterOn) {
            this.particleFilter.resetPosition(this.ball.getPosition(),
                this.ball.getSpeed());
          }
          this.ball
              .setUpdateMode(UPDATE_MODE.HEARED, this.world.getBodyCycle());
          break;

        // the ball was seen by the sender -> update when info is newer
        case COMPLETE_BALL:
        case INTERCEPT_BALL:

          if (this.world.getAge(this.ball) == 1) {
            final Player speaker = this.world.getExpectedPlayers()[psi.getNumber() - 1];
            if (speaker.getDistance(this.ball) > this.world.getSelfRef()
                .getDistance(this.ball) - 4) {
              continue;
            }
          }

          mf.decodeMessageNumber(i);
          this.ball.getPosition().copy(mf.getPosition());
          this.ball.getSpeed().copy(mf.getSpeed());
          if (mf.getAge() == 0 && this.world.getAge(this.ball) > 0) {
            this.ball.extrapolate(1);
          }
          this.ball.setReliable(true);
          this.ball.setSpeedReliable(true);
          this.ball.setBodyCycle(this.world.getBodyCycle());
          this.ball.setCycle(this.world.getCycle());
          if (this.ParticlefilterOn) {
            this.particleFilter.resetPosition(this.ball.getPosition(),
                this.ball.getSpeed());
          }
          this.ball
              .setUpdateMode(UPDATE_MODE.HEARED, this.world.getBodyCycle());
          break;

        // the ball was communicated by the fact that a dribbling player said
        // his position, as the player has the ball under control we set the
        // ball's velocity to "0" and it's position in front of the player
        case DRIBBLE_PPOS:
          mf.decodeMessageNumber(i);

          // ball is in front of the player
          this.ball.getPosition().copy(mf.getPositions()[0]);

          // set ball in front of player in direction of the others goal
          if (mf.getAge() == 0) {
            this.ball.getPosition().addToThis(new Vektor(-0.5, 0));
          }
          else {
            this.ball.getPosition().addToThis(new Vektor(0.5, 0));
          }

          // -> set speed to 0
          this.ball.getSpeed().reset();
          this.ball.setReliable(true);
          this.ball.setSpeedReliable(false);
          // if we dribble the next ball is sent (age code =1)
          // if an opponent dribbles the seen ball is sent (code = 0)
          this.ball.setBodyCycle(this.world.getBodyCycle() - 1 + mf.getAge());
          this.ball.setCycle(this.world.getCycle() - 1 + mf.getAge());
          if (this.ParticlefilterOn) {
            this.particleFilter.resetPosition(this.ball.getPosition(),
                this.ball.getSpeed());
          }
          this.ball
              .setUpdateMode(UPDATE_MODE.HEARED, this.world.getBodyCycle());
          break;

        default:
          ;
      }

    }
  }

  /**
   * This method handles Say-Messages from the coach.
   * 
   * @param csi -
   *          the Say-Information!
   */
  public void update(final CoachSayInfo csi) {

    final MessageFactory mf = this.world.getMessageFactory();
    final LinkedList<MESSAGES> types = mf.getMessageTypes();

    for (int i = 0; i < types.size(); i++) {
      final MESSAGES m = types.get(i);

      switch (m) {

        case TRAINER_BALL_POS:
          mf.decodeMessageNumber(i);
          this.updateBallPosFromCSI(mf.getAge(), mf.getPosition());
          break;

        case TRAINER_BALL_SPD:
          mf.decodeMessageNumber(i);
          this.updateBallSpdFromCSI(mf.getAge(), mf.getSpeed());
          break;

        case BALL_POS:
          mf.decodeMessageNumber(i);
          this.updateBallPosFromCSI(mf.getAge(), mf.getPosition());
          break;
      }
    }
  }

  /**
   * updates the ball after a play mode info. Some play_modes (i.e.
   * BEFORE_KICK_OFF) tell us where the ball is.
   * 
   * @param info -
   *          Info for the update
   */
  public void update(final PlayModeInfo aInfo) {

    switch (((PlayModeInfo) aInfo).getPlayModeType()) {
      case PLAY_ON:
        this.ball.setCatched(false);
        break;
      case PENALTY_SETUP_OWN:
      case PENALTY_SETUP_OTHER:
      case CORNER_KICK_OWN:
      case CORNER_KICK_OTHER:
      case GOAL_KICK_OWN:
      case GOAL_KICK_OTHER:
        this.ball.getSpeed().reset();
        break;

      case KICK_OFF_OWN:
      case KICK_OFF_OTHER:
        if (this.ball.getPosition().getDistance(new Vektor()) > 0.3) {
          this.ball.getPosition().reset();
        }
        this.ball.getSpeed().reset();
        break;

    }
  }

  /**
   * updates the ball after a referee info.
   * 
   * @param info -
   *          Info for the update
   */
  public void update(final RefereeInfo info) {

    switch (((RefereeInfo) info).getRefereeMessageType()) {
      case FOUL_OWN:
      case FOUL_OTHER:
      case PENALTY_FOUL_OWN:
      case PENALTY_FOUL_OTHER:
      case BACKPASS_OTHER:
      case BACKPASS_OWN:
      case CATCH_FAULT_OTHER:
      case CATCH_FAULT_OWN:
      case DROP_BALL:
      case FREE_KICK_FAULT_OTHER:
      case FREE_KICK_FAULT_OWN:
      case OFFSIDE_OTHER:
      case OFFSIDE_OWN:
      case HALF_TIME:
        this.ball.getSpeed().reset();
        this.ball.setCatched(false);
        break;

      case GOALIE_CATCH_BALL_OWN:
        this.ball.getSpeed().reset();
        this.ball.setCatched(true);
        break;
      case GOALIE_CATCH_BALL_OTHER:
        this.ball.setCatched(true);
        this.ball.setReliable(false);
        break;
    }
  }

  /**
   * <code>getSpeed</code>
   * 
   * @return The ball's velocity. Note that if you perform operations on this
   *         Vektor that change it, you need to clone it first!
   */
  public Vektor getSpeed() {

    return this.ball.getSpeed();
  }

  /*
   * (non-Javadoc)
   * 
   * @see robocup.component.worldobjects.DynamicObject#isReliable()
   */
  public boolean isReliable() {

    return this.ball.isReliable();
  }

  /*
   * (non-Javadoc)
   * 
   * @see robocup.component.worldobjects.StaticObject#getCycle()
   */
  public int getCycle() {

    return this.ball.getCycle();
  }

  /**
   * <code>getPosition</code>
   * 
   * @return Returns the ball's position. Note that if you perform operations on
   *         this Vektor that change it, you need to clone it first!
   */
  public Vektor getPosition() {

    return this.ball.getPosition();
  }

  /**
   * @return Returns the ball. (REF!!)
   */
  public Ball getBallRef() {

    return this.ball;
  }

  /**
   * the last seen ball.
   * 
   * @return
   */
  public Ball getLastSeenBall() {

    return this.lastSeenBall;
  }

  /**
   * This method acualizes the balls position after a players message, if he has
   * seen it more recently,
   * 
   * @param age
   * @param position
   */
  private void updateBallPosFromCSI(final int age,
      final Vektor position) {

    this.ball.setBodyCycle(this.world.getBodyCycle());
    this.ball.setCycle(this.world.getCycle());
    this.ball.setReliable(true);
    this.ball.getPosition().copy(position);
  }

  /**
   * This method acualizes the balls speed after a players message, if he has
   * seen it more recently,
   * 
   * @param age
   * @param position
   */
  private void updateBallSpdFromCSI(final int age,
      final Vektor speed) {

    this.ball.setBodyCycle(this.world.getBodyCycle());
    this.ball.setCycle(this.world.getCycle());
    this.ball.setReliable(true);
    this.ball.getSpeed().copy(speed);
    this.ball.setSpeedReliable(true);
  }

  /**
   * resets the particle filter in casewe collided with the ball.
   * 
   */
  public void resetFilterAfterCollisionWithBall() {

    this.particleFilter.resetPosition(this.ball.getPosition(), this.ball
        .getSpeed());
  }

  /* (non-Javadoc)
   * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.PlayerTypesInfo)
   */
  public void update(final PlayerTypesInfo info) {

    // TODO Auto-generated method stub
    
  }

}
